package com.test.particles;

import javax.vecmath.Point2f;
import javax.vecmath.Point2i;
import javax.vecmath.Point3f;
import javax.vecmath.Vector2f;

public class Particle {
	Particle(Point2f pos)
	{
		mPos = pos;
		
		mVel = new Vector2f(0.0f, 0.0f);
		mAccel = new Vector2f(0.0f, 0.0f);		
	}
	
	Particle(float x, float y)
	{
		mPos = new Point2f(x, y);
		
		mVel = new Vector2f(0.0f, 0.0f);
		mAccel = new Vector2f(0.0f, 0.0f);		
	}
	
	public void addVel(Vector2f vel)
	{
		mVel.add(vel);
		
	}
	
	public void addAccel(Vector2f vel)
	{
		mAccel.add(vel);
	}
	
	public void resetAccel()
	{
		mAccel.x = 0.0f;
		mAccel.y = 0.0f;
	}
	
	public Point3f getColor()
	{
		return mColor;
	}
	
	public void setColor(Point3f color)
	{
		mColor = color;
	}
	
	public void integrate(float dt) {
		Point2f newAccel = new Point2f(mAccel);
		newAccel.scale(dt);
		mVel.add(newAccel);
	
		Point2f newVel = new Point2f(mVel);
		newVel.scale(dt);
		mPos.add(newVel);
	}
	
   public void integrateRK4(float t, float dt)
   {
		Point2f dp1 = new Point2f(0.0f, 0.0f);
		Point2f dv1 = new Point2f(0.0f, 0.0f);
		
		Point2f tp1 = new Point2f(0.0f, 0.0f);
		Point2f tp2 = new Point2f(0.0f, 0.0f);
		
		evaluate(mPos, mVel, t, 0.0f, tp1, tp2, dp1, dv1);

		Point2f dp2 = new Point2f(0.0f, 0.0f);
		Point2f dv2 = new Point2f(0.0f, 0.0f);
		evaluate(mPos, mVel, t+dt*0.5f, dt*0.5f, dp1, dv1, dp2, dv2);
		Point2f dp3 = new Point2f(0.0f, 0.0f);
		Point2f dv3 = new Point2f(0.0f, 0.0f);
		evaluate(mPos, mVel, t+dt*0.5f, dt*0.5f, dp2, dv2, dp3, dv3);	
		Point2f dp4 = new Point2f(0.0f, 0.0f);
		Point2f dv4 = new Point2f(0.0f, 0.0f);
		evaluate(mPos, mVel, t+dt, dt, dp3, dv3, dp4, dv4);	   	

		Point2f dpdt = new Point2f(0.0f, 0.0f);
		dpdt.x = 1.0f/6.0f * (dp1.x + 2.0f*(dp2.x + dp3.x) + dp4.x);
		dpdt.y = 1.0f/6.0f * (dp1.y + 2.0f*(dp2.y + dp3.y) + dp4.y);
		Point2f dvdt = new Point2f();
		dvdt.x = 1.0f/6.0f * (dv1.x + 2.0f*(dv2.x + dv3.x) + dv4.x);
		dvdt.y = 1.0f/6.0f * (dv1.y + 2.0f*(dv2.y + dv3.y) + dv4.y);
		
		dpdt.scale(dt);
		mPos.add(dpdt);
		 
		dvdt.scale(dt);
		mVel.add(dvdt);
    }
   
   // p is initial pos
   // v is initial vel
   // t is currenttime
   // dt is deltatime
   // dp is derivative pos
   // dv is derivative vel
   // outp is return pos
   // outv is return vel
    public void evaluate(Point2f p, Vector2f v, float t, float dt, Point2f dp, Point2f dv, Point2f outp, Point2f outv)
    {
         Point2f np = new Point2f(p);
         Vector2f nv = new Vector2f(v);
         
         Point2f ep = new Point2f(dp);
         ep.scale(dt);
         
         np.add(ep);
         
         Point2f ev = new Point2f(dv);
         ev.scale(dt);
         
         nv.add(ev);
         

         outp.x = np.x;
         outp.y = np.y;
         float nt = t;
         nt += dt;

         outv = new Point2f(0.0f, -2.0f);

    }
    
    public void accelerate(Point2f p, Point2f v, float t, Point2f out)
    {
		Point2f sp = new Point2f(0.0f, 1.0f);
		out.x = sp.x;
		out.y = sp.y; 	
    }
	
    
    
    // get/set state
	public Point2f getPos()
	{
		return mPos;
	}
	
	public void setPos(Point2f pos)
	{
		mPos = pos;
	}
	
	public Vector2f getVel()
	{
		return mVel;
	}
	
	public void setVel(Point2f vel)
	{
		mVel.x = vel.x;
		mVel.y = vel.y;
	}
	
	public void setVel(Vector2f vel)
	{
		mVel = vel;
	}
	
	public Vector2f getAccel()
	{
		return mAccel;
	}
	
	public void setAccel(Vector2f accel)
	{
		mAccel = accel;
	}
	
	public void setCell(Point2i cell)
	{
		mCell = cell;
	}
	
	public Point2i getCell()
	{
		return mCell;
	}
	
	// state variables
	private Point2f mPos; 
	private Vector2f mVel;
	private Vector2f mAccel;
	private Point3f mColor;
	private Point2i mCell;
}
